import numpy as np

# 常量
RAD2DEG = 57.2957795
DEG2RAD = 0.0174533
THR_HOVER = 0.609

# 初始条件
ModelInit_PosE = np.array([0, 0, 0])
ModelInit_VelB = np.array([0, 0, 0])
ModelInit_AngEuler = np.array([0, 0, 0])
ModelInit_RateB = np.array([0, 0, 0])
ModelInit_Rads = 557.142

# 控制参数
# 姿态PID参数
Kp_RP_ANGLE = 8.5#6.5
Kp_RP_AngleRate = 0.1#0.1
Ki_RP_AngleRate = 0.04
Kd_RP_AngleRate = 0.002

Kp_YAW_AngleRate = 0.7
Ki_YAW_AngleRate = 0.02
Kd_YAW_AngleRate = 0.002
# 位置PID参数
Kpxp = 0.60#2.5#0.8
Kpyp = 0.60#2.5#0.8
Kpzp = 0.60#5.5#1.5#0.8
Kvxp = 5.5#5.5#2.5#2.0#1.5
Kvxi = 0.02#0.8#0.5
Kvxd = 0.1#0.6#0.2#0.1
Kvyp = 5.5#5.5#2.5#2.0#1.5
Kvyi = 0.02#0.8#0.5
Kvyd = 0.1#0.6#0.2#0.1
Kvzp = 0.45
Kvzi = 0.01
Kvzd = 0.008
# 积分饱和
Saturation_I_RP_Max = 0.3
Saturation_I_RP_Min = -0.3
Saturation_I_Y_Max = 0.2
Saturation_I_Y_Min = -0.2

# 控制约束
MAX_CONTROL_ANGLE_ROLL = 35
MAX_CONTROL_ANGLE_PITCH = 35
MAX_CONTROL_ANGLE_RATE_PITCH = 220
MAX_CONTROL_ANGLE_RATE_ROLL = 220
MAX_CONTROL_ANGLE_RATE_Y = 200

# 无人机模型参数
ModelParam_uavMass = 1.4
ModelParam_uavJxx = 0.0211
ModelParam_uavJyy = 0.0219
ModelParam_uavJzz = 0.0366
ModelParam_uavJ = np.diag([ModelParam_uavJxx, ModelParam_uavJyy, ModelParam_uavJzz])
ModelParam_uavType = 3
ModelParam_uavMotNumbs = 4
ModelParam_uavR = 0.225
ModelParam_uavCd = 0.073
ModelParam_uavCCm = np.array([0.01, 0.01, 0.0055])

ModelParam_motorCr = 1148
ModelParam_motorWb = -141.4
ModelParam_motorT = 0.02
ModelParam_motorJm = 0.0001287
ModelParam_motorMinThr = 0.05
ModelParam_rotorCm = 1.779e-07
ModelParam_rotorCt = 1.105e-05

# 环境参数
ModelParam_envGravityAcc = 9.8
ModelParam_envLongitude = 116.259368300000
ModelParam_envLatitude = 40.1540302
ModelParam_GPSLatLong = [ModelParam_envLatitude, ModelParam_envLongitude]
ModelParam_envAltitude = -41.5260009765625
ModelParam_BusSampleRate = 0.001

def get_control_parameters():
    return {
        'RAD2DEG': RAD2DEG,
        'DEG2RAD': DEG2RAD,
        'THR_HOVER': THR_HOVER,
        'ModelInit_PosE': ModelInit_PosE,
        'ModelInit_VelB': ModelInit_VelB,
        'ModelInit_AngEuler': ModelInit_AngEuler,
        'ModelInit_RateB': ModelInit_RateB,
        'ModelInit_Rads': ModelInit_Rads,
        'Kp_RP_ANGLE': Kp_RP_ANGLE,
        'Kp_RP_AngleRate': Kp_RP_AngleRate,
        'Ki_RP_AngleRate': Ki_RP_AngleRate,
        'Kd_RP_AngleRate': Kd_RP_AngleRate,
        'Kp_YAW_AngleRate': Kp_YAW_AngleRate,
        'Ki_YAW_AngleRate': Ki_YAW_AngleRate,
        'Kd_YAW_AngleRate': Kd_YAW_AngleRate,
        'Saturation_I_RP_Max': Saturation_I_RP_Max,
        'Saturation_I_RP_Min': Saturation_I_RP_Min,
        'Saturation_I_Y_Max': Saturation_I_Y_Max,
        'Saturation_I_Y_Min': Saturation_I_Y_Min,
        'MAX_CONTROL_ANGLE_ROLL': MAX_CONTROL_ANGLE_ROLL,
        'MAX_CONTROL_ANGLE_PITCH': MAX_CONTROL_ANGLE_PITCH,
        'MAX_CONTROL_ANGLE_RATE_PITCH': MAX_CONTROL_ANGLE_RATE_PITCH,
        'MAX_CONTROL_ANGLE_RATE_ROLL': MAX_CONTROL_ANGLE_RATE_ROLL,
        'MAX_CONTROL_ANGLE_RATE_Y': MAX_CONTROL_ANGLE_RATE_Y,
        'ModelParam_uavMass': ModelParam_uavMass,
        'ModelParam_uavJ': ModelParam_uavJ,
        'ModelParam_uavType': ModelParam_uavType,
        'ModelParam_uavMotNumbs': ModelParam_uavMotNumbs,
        'ModelParam_uavR': ModelParam_uavR,
        'ModelParam_uavCd': ModelParam_uavCd,
        'ModelParam_uavCCm': ModelParam_uavCCm,
        'ModelParam_motorCr': ModelParam_motorCr,
        'ModelParam_motorWb': ModelParam_motorWb,
        'ModelParam_motorT': ModelParam_motorT,
        'ModelParam_motorJm': ModelParam_motorJm,
        'ModelParam_motorMinThr': ModelParam_motorMinThr,
        'ModelParam_rotorCm': ModelParam_rotorCm,
        'ModelParam_rotorCt': ModelParam_rotorCt,
        'ModelParam_envGravityAcc': ModelParam_envGravityAcc,
        'ModelParam_GPSLatLong': ModelParam_GPSLatLong,
        'ModelParam_envAltitude': ModelParam_envAltitude,
        'ModelParam_BusSampleRate': ModelParam_BusSampleRate,
        'Kpxp':Kpxp,
        'Kpyp':Kpyp,
        'Kpzp':Kpzp,
        'Kvxp':Kvxp,
        'Kvxi':Kvxi,
        'Kvxd' : Kvxd,
        'Kvyp' :Kvyp,
        'Kvyi' :Kvyi,
        'Kvyd' :Kvyd,
        'Kvzp' :Kvzp,
        'Kvzi' :Kvzi,
        'Kvzd' :Kvzd
    }
